#include "all.h"




int main98()
{
	//vector<int> a,b;
	//a.push_back(1);
	//a.push_back(2);
	//a.push_back(3);

	//for(int i = 0;i<a.size();i++) {	cout<<a[i];	} cout<<endl;
	//b=vector<int>(a);
	//for(int i = 0;i<b.size();i++) {	cout<<b[i];	} cout<<endl;

	//for(int i = 0;i<a.size();i++) {	cout<<a[i];	} cout<<endl;

	//a.push_back(50);

	//for(int i = 0;i<a.size();i++) {	cout<<a[i];	} cout<<endl;
	//for(int i = 0;i<b.size();i++) {	cout<<b[i];	} cout<<endl;
 

	Mat I0,I,contours;
	Mat foreground;
	
	BackgroundSubtractorMOG2 mog;
	//VideoCapture vc("C:\\Users\\domenico\\Dropbox\\tesi\\matlab\\movie\\visor_1206627910990_video_1.avi");
	//VideoCapture vc("C:\\Users\\domenico\\Dropbox\\tesi\\matlab\\movie\\Visor_0000072.avi");
	VideoCapture vc("C:\\Users\\domenico\\Dropbox\\tesi\\matlab\\movie\\gmm.avi");
	//VideoCapture vc("C:\\Users\\domenico\\Dropbox\\tesi\\matlab\\movie\\traffic2.avi");
	//VideoCapture vc(0);
	//VideoCapture vc("C:\\Users\\domenico\\Dropbox\\tesi\\matlab\\movie\\dance.avi");
	//vc.set(CV_CAP_PROP_FRAME_WIDTH, 160);
	//vc.set(CV_CAP_PROP_FRAME_HEIGHT, 120);
	//Visor_0000072

	int LABEL_COUNT=0;
	int SOGLIA_AREA = 200;   //tuning
	float learningrate=0.0001; //tuning
	int max_count=50; //tuning
	double qLevel =0.01; //tuning
	double minDist = 10; //tuning

	
	vector<Point2f> points[2];
	vector<RotatedRect> blobs;
	vector<vector<Point2f>> history;
	int HISTORY_SIZE=100;
	vector<Point2f> features,initial;
	


	vector<uchar> status; // status of tracked features
    vector<float> err;

	//OneLeaveShop2cor
	 vc>>I0;
	 //baro leggermente sul primo frame per creare il background :) 
	 Mat diagram(I0.rows,I0.cols,CV_8U);
	 diagram = Mat::zeros(I0.rows,I0.cols,CV_8U);
	 
	 

	 for(int i = 0 ;i<1000 ; i++)
	 {
		 mog(I,foreground,learningrate);
	 }

	 Mat prev,current;
	 cvtColor(I0,prev,CV_BGR2GRAY);
	 prev.copyTo(current);
	 //vector<Point2f> points_c,points_p;

	// cout<<"before goodfeaturestotrack"<<endl;
	// goodFeaturesToTrack(current,points_c,max_count,qLevel,minDist);
	 //goodFeaturesToTrack(prev,points_p,max_count,qLevel,minDist);
	// cout<<"start while"<<endl;

	 int frNum=0;
	while(I0.data)
	{
		
		
		mog(current,foreground,learningrate);
		mog.set("detectShadows", true); 
		 
		//foreground = imfill(foreground,'holes');
		imshow("Foreground",foreground);
		imshow("Original",current);
		
		cv::dilate(foreground,foreground,cv::Mat());
		cv::erode(foreground,foreground,cv::Mat());

		/*double min;
		double max;
		minMaxLoc(foreground,&min,&max);
		cout<<max<<endl;*/
		
		threshold(foreground,foreground,128,255,CV_THRESH_BINARY);
		imshow("Foreground (2)",foreground);
		//cout<<foreground;
		//system("pause");

		 

		 
		vector<vector<Point> > contours,contours2;
		vector<Vec4i> hierarchy;
		
		Mat I1,I2;
		I0.copyTo(I1);
		I0.copyTo(I2);
		
		//find contourn


		
		findContours(foreground,contours,hierarchy,CV_RETR_TREE,CV_CHAIN_APPROX_SIMPLE);

		vector<Point>hull;
		for(int i = 0;i<contours.size();i++)
		{
			double area = contourArea(contours[i]);
			if(area>SOGLIA_AREA)
			{
				vector<Point>hulltmp;
				contours2.push_back(contours[i]);
				convexHull(contours[i],hulltmp);
				//for(int j=0;j<hulltmp.size();j++){
				//circle(I0,hulltmp[j],1,Scalar(0,255,0));
				//}

				hull.insert(hull.end(),hulltmp.begin(),hulltmp.end());

			}
		}

		for(int j=0;j<hull.size();j++){
				circle(I0,hull[j],1,Scalar(0,255,0));
				}
		

		

		//cout<<contours2.size()<<" su "<<contours.size()<<endl;
		drawContours(I0,contours2,-1,Scalar(255,0,0));

		

		
		imshow("Contorni",I0);

		//tracking
		 
		if(points[0].size()<20)
		{
			goodFeaturesToTrack(current,features,max_count,qLevel,minDist);
			points[0].insert(points[0].end(),features.begin(),features.end());
			initial.insert(initial.end(),features.begin(),features.end());
		//	Vector<int> o =Vector<int>(features.size());
			//for( int j=0;j<o.size();j++)
		//	{
			//	o[j]=-1;
		//	}
			 

			
		 //   label.insert(label.end(),o.begin(),o.end());
		}
		//cout<<"XXX1"<<endl;

		calcOpticalFlowPyrLK(prev,current,points[0],points[1],status,err);
		//cout<<"XXX1.0"<<endl;
		
		int k = 0;
		for(int i=0;i<points[1].size();i++)
		{
			//cout<<status[i]<<endl;
			if(status[i] && (abs(points[0][i].x-points[1][i].x)+abs(points[0][i].y-points[1][i].y))>2)
			{
				//cout<<"Status [ ok ]"<<endl;
				
				

				initial[k]=initial[i];
				points[1][k]=points[1][i];
				circle(I1,points[1][i],1,Scalar(255,0,0));
				circle(I1,initial[i],3,Scalar(0,0,255));
				line(I1,initial[i],points[1][i],Scalar(0,255,0));

				
				k++;

			}
			else
			{ //  cout<<"Status [ NO ]"<<endl;
				//circle(I1,points[1][i],1,Scalar(0,0,255));
			}
		}
		
		points[1].resize(k);
		initial.resize(k);
		//label.resize(k);
		
		imshow("Points",I1);
		
	
		if(history.size()<HISTORY_SIZE)
			history.push_back(points[1]);
		else		
			history[frNum%HISTORY_SIZE]=points[1];

		
		for(int i=frNum%HISTORY_SIZE;i<(frNum%HISTORY_SIZE)+history.size();i++)
		{
			for(int j = 0;j<history[i%history.size()].size();j++)
				{		
					 
					//Moments m=cv::moments(history[i%history.size()]);
					//double mx = m.m10/m.m00;
					//double my = m.m01/m.m00;
					//double orientation = .5*atan(2*(m.m11/m.m00)/(   (m.m20/m.m00)-(m.m02/m.m00)  )  );

					//circle(I0,Point(mx,my),1,Scalar(255,0,0));
					circle(I0,history[i%history.size()][j],1,Scalar(255,0,255));
				}
			
			for(int j = 0;j<history[i%history.size()].size();j++)
			{		
				
				circle(I0,history[i%history.size()][j],1,Scalar(255,0,255));
			}
		}
			
		imshow("Flow",I0);
		imshow("Diagram",diagram);
		frNum++;


		

		blobs.clear();//--------------------------------- reset
		int u = 0;
		for(int i = 0;i<contours2.size();i++)
		{
			 
			
			RotatedRect rr = fitEllipse(contours2[i]);
			rectangle(I0,rr.boundingRect(),Scalar(255,0,0));
			circle(I0,rr.center,1,Scalar(255,255,255));
			ellipse(I0,rr,Scalar(128,255,255));
			ellipse(I2,rr,Scalar(128,255,255));

			blobs.push_back(rr);

		}
		imshow("RotatedRect",I0);
		
		vector<int> label;
		
		///label.insert(label.end(),points[1].begin(),points[1].end());
		 
		for(int i=0;i<points[1].size();i++)
		{
			int minIdx=-1;
			double minDist=1000000;
			for(int j=0;j<contours2.size();j++)
			{
				double dist = pointPolygonTest(contours2[j],points[1][i],true);
				if(dist<minDist)
				{
					minDist = dist;
					minIdx = j;
				}							
			}

			cout<<minDist<<endl;
			label.push_back(minIdx);
			
			if(label[i]>-1)
			{
				circle(I2,points[1][i],1,Scalar(label[i]*3+40,255,label[i]*10));
			}else
			{
				circle(I2,points[1][i],1,Scalar(0,0,255));
			}

		}
		cout<<"Size label: "<<label.size()<<endl;
		imshow("Blobs",I2);

		 
		
	




		waitKey(30);


		points[1].swap(points[0]);
		current.copyTo(prev);
		vc>>I0;
		cvtColor(I0,current,CV_BGR2GRAY);
	}

		
	return 1;

	//waitKey(0);
}






 